#ifndef ROBORTS_COSTMAP_FRIEND_LAYER_H
#define ROBORTS_COSTMAP_FRIEND_LAYER_H

#include <ros/ros.h>
#include "layer.h"
#include "layered_costmap.h"
#include "costmap_layer.h"
#include "costmap_2d.h"

namespace roborts_costmap
{
 
class FriendLayer : public Layer, public Costmap2D
{
public:
  FriendLayer(std::string map_name_);

  void  FlayerInfoCallback(const geometry_msgs::PoseStamped :: ConstPtr&msg);
  void OnInitialize();
  void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,double* max_y);
  void UpdateCosts(roborts_costmap::Costmap2D& master_new, int min_i, int min_j, int max_i, int max_j);
  bool isDiscretized()
  {
    return true;
  }
 
void MatchSize();
private:
  ros::Subscriber flayer_sub_;
  geometry_msgs::PointStamped friend_area_map;
  geometry_msgs::PointStamped friend_area;
  std::vector<std::pair<unsigned int,unsigned int>> friend_cells;
  std::string map_name_;
  unsigned  flag;
  double roll,pitch,yaw;
  double length,width;
};
}
#endif